#ifndef UR_CONTROL_HPP
#define UR_CONTROL_HPP

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <pinocchio/parsers/sample-models.hpp>
#include <pinocchio/spatial/explog.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/joint-configuration.hpp>
#include <pinocchio/parsers/urdf.hpp>

#include <serial_driver/serial_driver.hpp>

#include "ur_control/loop.hpp"

#include <chrono> // 引入时间相关头文件
// 使用时间单位的字面量，可以在代码中使用 s 和 ms 表示时间
using namespace std::chrono_literals;

class ur_control : public rclcpp::Node
{
private:
    std::string controller_name_ = "ur_position_controller"; // 控制器名称
    rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr robot_command_publisher_;
    rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr robot_state_subscriber_;
    //读取urdf模型
    const std::string urdf_filename_ = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/../ur_description/urdf/ur5e.urdf";
    // pinocchio模型
    pinocchio::Model ur_model_;

    // 目标位姿
    Eigen::Quaterniond target_quaterniond_;
    Eigen::Vector3d target_position_;

    // 关节角度
    std::vector<double> joints_now_pos_{0, 0, 0, 0, 0, 0};
    std::vector<double> joints_command_pos_{0, 0, 0, 0, 0, 0};

    // 定时器
    rclcpp::TimerBase::SharedPtr timer_;

    // loop
    std::shared_ptr<LoopFunc> loop_keyboard_;
    std::shared_ptr<LoopFunc> loop_serial_;

    // 串口
    std::shared_ptr<drivers::serial_driver::SerialDriver> serial_driver_;
    std::shared_ptr<drivers::common::IoContext> io_context_;

    // 逆运动学解算函数
    void InverseKinematics(Eigen::Quaterniond target_quater, Eigen::Vector3d target_pos, pinocchio::Model model);
    // 发布控制命令函数
    void publish_commands();
    // 获取关节状态函数
    void get_joint_states(const sensor_msgs::msg::JointState::SharedPtr joint_states);
    // 键盘控制接口
    void KeyboardInterface();
    // 串口初始化函数
    void serial_init(void);
    // 串口读取目标位置和姿态
    void serial_get_target(void);

public:
    ur_control();
    ~ur_control();
};

#endif